////////YUV CODEBOOK
// Based on the implementation from Learning OpenCV by Oreilly. 
//Gary Bradski, July 14, 2005


#include "background_codebook_header.h"

//GLOBALS FOR ALL CAMERA MODELS

//For connected components:
int CVCONTOUR_APPROX_LEVEL = 2;   // Approx.threshold - the bigger it is, the simpler is the boundary
int CVCLOSE_ITR = 1;				// How many iterations of erosion and/or dialation there should be
//#define CVPERIMSCALE 4			// image (width+height)/PERIMSCALE.  If contour lenght < this, delete that contour

//For learning background

//Just some convienience macros
#define CV_CVX_WHITE	CV_RGB(0xff,0xff,0xff)
#define CV_CVX_BLACK	CV_RGB(0x00,0x00,0x00)

int findBiggestContourIndex(vector<vector<Point>>&contours, int minimPerimeter)
{
	int indexOfContour = -1;
	int sizeOfBiggestContour = minimPerimeter;
	for(int k = 0; k < contours.size();k++)
	{
		if(contours[k].size() > sizeOfBiggestContour)
		{
			sizeOfBiggestContour = contours[k].size();
			indexOfContour = k;
		}
	}
	return indexOfContour;
}

int updateCodeBook(uchar *p, codeBook *cb , int k, int numChannels)
{

	codeBook &c = cb[k];
	if(c.numEntries == 0)
	{
		c.t = 0;
	}
	c.t += 1;		//Record learning event
	//SET HIGH AND LOW BOUNDS
	int n;
	unsigned int high[3],low[3];
	for(n=0; n<numChannels; n++)
	{
		high[n] = *(p+n) + *(cb->cbBounds+n);
		if(high[n] > 255) 
		{
			high[n] = 255;
		}
		low[n] = *(p+n)- *(cb->cbBounds+n);
		if(low[n] < 0)
		{
			low[n] = 0;
		}
	}
	int matchChannel;
	//SEE IF THIS FITS AN EXISTING CODEWORD
	int i;
	for(i=0; i<c.numEntries; i++)
	{
		matchChannel = 0;
		for(n=0; n < numChannels; n++)
		{
			if((c.cb[i]->learnLow[n] <= *(p+n)) && (*(p+n) <= c.cb[i]->learnHigh[n])) //Found an entry for this channel
			{
				matchChannel++;
			}
		}
		if(matchChannel == numChannels) //If an entry was found over all channels
		{
			c.cb[i]->t_last_update = c.t;
			//adjust this codeword for the first channel
			for(n=0; n<numChannels; n++)
			{
				if(c.cb[i]->max[n] < *(p+n))
				{
					c.cb[i]->max[n] = *(p+n);
				}
				else if(c.cb[i]->min[n] > *(p+n))
				{
					c.cb[i]->min[n] = *(p+n);
				}
			}
			break;
		}
	}

	//OVERHEAD TO TRACK POTENTIAL STALE ENTRIES
	for(int s=0; s < c.numEntries; s++)
	{
		//This garbage is to track which codebook entries are going stale
		int negRun = c.t - c.cb[s]->t_last_update;
		if(c.cb[s]->stale < negRun) 
		{	
			c.cb[s]->stale = negRun;
		}
	}


	//ENTER A NEW CODE WORD IF NEEDED
	if(i == c.numEntries)  //No existing code word found, make a new one
	{
		codeWord **foo = new codeWord* [c.numEntries+1];
		for(int ii=0; ii<c.numEntries; ii++)
		{
			foo[ii] = c.cb[ii];
		}
		foo[c.numEntries] = new codeWord;
		if(c.numEntries)
		{
			delete [] c.cb;
		}
		c.cb = foo;
		for(n=0; n < numChannels; n++)
		{
			c.cb[c.numEntries]->learnHigh[n] = high[n];
			c.cb[c.numEntries]->learnLow[n] = low[n];
			c.cb[c.numEntries]->max[n] = *(p+n);
			c.cb[c.numEntries]->min[n] = *(p+n);
		}
		c.cb[c.numEntries]->t_last_update = c.t;
		c.cb[c.numEntries]->stale = 0;
		c.numEntries += 1;
	}

	//SLOWLY ADJUST LEARNING BOUNDS
	for(n=0; n < numChannels; n++)
	{
		if(c.cb[i]->learnHigh[n] < high[n]) 
		{
			c.cb[i]->learnHigh[n] += 1;
		}
		if(c.cb[i]->learnLow[n] > low[n]) 
		{
			c.cb[i]->learnLow[n] -= 1;
		}
	}
	return(i);
}

///////////////////////////////////////////////////////////////////////////////////
// uchar cvbackgroundDiff(uchar *p, codeBook &c, int minMod, int maxMod)
// Given a pixel and a code book, determine if the pixel is covered by the codebook
//
// p		pixel pointer (YUV interleaved)
// c		codebook reference
// numChannels  Number of channels we are testing
// maxMod	Add this (possibly negative) number onto max level when code_element determining if new pixel is foreground
// minMod	Subract this (possible negative) number from min level code_element when determining if pixel is foreground
//
// NOTES:
// minMod and maxMod must have length numChannels, e.g. 3 channels => minMod[3], maxMod[3].
//
// Return
// 0 => background, 255 => foreground
uchar computeBackgroundDiff(uchar *p, codeBook *cb, int k, int numChannels)
{
	int matchChannel;

	//SEE IF THIS FITS AN EXISTING CODEWORD
	int i;
	codeBook &c = cb[k];
	for(i=0; i<c.numEntries; i++)
	{
		matchChannel = 0;
		for(int n=0; n<numChannels; n++)
		{
			if((c.cb[i]->min[n] - cb->modMin[n] <= *(p+n)) && (*(p+n) <= c.cb[i]->max[n] + cb->modMax[n]))
			{
				matchChannel++; //Found an entry for this channel
			}
			else
			{
				break;
			}
		}
		if(matchChannel == numChannels)
		{
			break; //Found an entry that matched all channels
		}
	}
	if(i >= c.numEntries)
	{
		return(255);
	}
	return(0);
}


//UTILITES/////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////
//int clearStaleEntries(codeBook &c)
// After you've learned for some period of time, periodically call this to clear out stale codebook entries
//
//c		Codebook to clean up
//
// Return
// number of entries cleared
int clearStaleEntries(codeBook &c)
{
	int staleThresh = c.t>>1;
	int *keep = new int [c.numEntries];
	int keepCnt = 0;
	//SEE WHICH CODEBOOK ENTRIES ARE TOO STALE
	for(int i=0; i<c.numEntries; i++)
	{
		if(c.cb[i]->stale > staleThresh)
			keep[i] = 0; //Mark for destruction
		else
		{
			keep[i] = 1; //Mark to keep
			keepCnt += 1;
		}
	}
	//KEEP ONLY THE GOOD
	c.t = 0;						//Full reset on stale tracking
	codeWord **foo = new codeWord* [keepCnt];
	int k=0;
	for(int ii=0; ii<c.numEntries; ii++)
	{
		if(keep[ii])
		{
			foo[k] = c.cb[ii];
			foo[k]->stale = 0;		//We have to refresh these entries for next clearStale
			foo[k]->t_last_update = 0;
			k++;
		}
	}
	//CLEAN UP
	delete [] keep;
	delete [] c.cb;
	c.cb = foo;
	int numCleared = c.numEntries - keepCnt;
	c.numEntries = keepCnt;
	return(numCleared);
}


void processMask(Mat& fgMask)
{
	Mat element = getStructuringElement(1, Size(5,5), Point(2,2)); 
	morphologyEx(fgMask, fgMask, MORPH_OPEN, element);
	morphologyEx(fgMask, fgMask, MORPH_CLOSE, element);
}

